#include "FeatureDetection.h"
#include <opencv2/nonfree/features2d.hpp>
#include <QDebug>
#include <algorithm>

const std::string FeatureDetection::DetectorTypeFAST = "FAST";
const std::string FeatureDetection::DetectorTypeSTAR = "STAR";
const std::string FeatureDetection::DetectorTypeSURF = "SURF";
const std::string FeatureDetection::DetectorTypeORB = "ORB";
const std::string FeatureDetection::DetectorTypeMSER = "MSER";
const std::string FeatureDetection::DetectorTypeGFFT = "GFFT";
const std::string FeatureDetection::DetectorTypeHARRIS = "HARRIS";
const std::string FeatureDetection::DetectorTypeDense = "Dense";
const std::string FeatureDetection::DetectorTypeSimpleBlob = "SimpleBlob";

FeatureDetection::FeatureDetection(const std::string &detectorType) : Filter(2,2)
{
	featureDetector = new cv::SurfFeatureDetector();
}


FeatureDetection::~FeatureDetection(void)
{
}

void FeatureDetection::Process()
{
	std::shared_ptr< std::vector<cv::KeyPoint> > keypoints = std::shared_ptr<std::vector<cv::KeyPoint> >(new std::vector<cv::KeyPoint>());
	featureDetector->detect(*inputs[0], *keypoints, *inputs[1]);

	//copy to cv::Mat
	outputs[0] = cv::Mat(keypoints->size(), 7, CV_32FC1);
	outputs[1] = cv::Mat(keypoints->size(), 2, CV_32SC1);

	for (int i = 0; i < keypoints->size(); ++i)
	{
		float *ptr = outputs[0].ptr<float>(i);
		int *pointPtr = outputs[1].ptr<int>(i);

		cv::KeyPoint kp = keypoints->at(i);

		int *intp;

		ptr[0] = kp.pt.x;
		ptr[1] = kp.pt.y;
		ptr[2] = kp.size;
		ptr[3] = kp.angle;
		ptr[4] = kp.response;
		intp = reinterpret_cast<int*>(&kp.octave);
		ptr[5] = *intp;
		intp = reinterpret_cast<int*>(&kp.class_id);
		ptr[6] = *intp;

		pointPtr[0] = kp.pt.x + 0.5f;
		pointPtr[1] = kp.pt.y + 0.5f;
	}
}
